#include "StdAfx.h"
#include "ContourDot.h"
#include "Utilities.h"
#include "Calculator.h"
#include <GdiPlus.h>

#define _USE_MATH_DEFINES
#include <cmath>

#ifdef max
#undef max
#undef min
#endif


////////////////////////////////////////////////
// Constants
const float kPrecision = 0.01f;
const float kApproximationPrecision = 0.8f;
const float kApproximationPrecisionSmall = 0.2f;

//////////////////////////////////////////////

ContourDot::ContourDot(int inX, int inY)
	: mCenterX(float(inX)), mCenterY(float(inY)), 
	mVx(0.0f), mVy(1.0f)
{
}
bool ContourDot::PlaceAtInitialPosition(Calculator& ioCalculator)
{
	const SubpixelInfo* p = ioCalculator.GetInfo(int(mCenterX), int(mCenterY));
	mA = p->a;
	mB = p->b;
	if (mA >= 0 && mB < 0)
	{
		mVx = -1; mVy = 0;
	}
	mCurvature = 0;
	CalculateTangentVector();
	ioCalculator.SetInitialDefaultBounds();
	if (!ioCalculator.UpdateCache(mCenterX, mCenterY, mVx, mVy, true) ||
		!ioCalculator.CalculateF(mA, mB))
	{
		return false;
	}
	
	ioCalculator.AdjustRectExceptFront();
	ioCalculator.AdjustRectExceptBack();

	bool result = ioCalculator.ApproximateByParabola(true);
	mApproximationPrecision = ioCalculator.GetLastApproximationEpsilon();
	bool wasCutAtConcaveXi = false;

	if (!result || kApproximationPrecisionSmall < mApproximationPrecision)
	{
		float a, b;
		ioCalculator.SummarizeABAndCalculateCurvature(a, b, mCurvature);

		float angle = mCurvature * ioCalculator.GetMaxEta();
		if (::fabs(angle) < 0.2f)
		{
			mA = a;
			mB = b;
			CalculateTangentVector();
			ioCalculator.SetInitialDefaultBounds();
		}
		else
		{
			// make smaller rectangle
			float halfWidth = std::max(1.0f, 0.2f / mCurvature);
			ioCalculator.SetInitialBounds(halfWidth, mCurvature);
		}

		if (!ioCalculator.UpdateCache(mCenterX, mCenterY, mVx, mVy) ||
			!ioCalculator.CalculateF(mA, mB))
		{
			return false;
		}
		ioCalculator.AdjustRectExceptFront();
		ioCalculator.AdjustRectExceptBack();
		result = ioCalculator.ApproximateByParabola(true);
		if (!result || 
			kApproximationPrecisionSmall < ioCalculator.GetLastApproximationEpsilon())
		{
			ioCalculator.CutAtTheConcaveXi();
			wasCutAtConcaveXi = true;
			result = ioCalculator.ApproximateByParabola(true);
		}
		mApproximationPrecision = ioCalculator.GetLastApproximationEpsilon();
		if (!result || kApproximationPrecision < mApproximationPrecision)
		{
			return false;
		}
	}
	MoveByXi(ioCalculator.GetDeltaXi());

	if (!result || mApproximationPrecision > kApproximationPrecision ||
		kOverrideOtherStroke == ioCalculator.HandleOverrides(mCenterX, mCenterY))
	{
		return false;
	}
	
	ioCalculator.SummarizeABAndCalculateCurvature(mA, mB, mCurvature);
	CalculateTangentVector();

	// one more iteration
	mOuterHalfsize = ioCalculator.GetDotHalfsize();
	if (0 >= mOuterHalfsize)
	{
		return false;
	}
	ioCalculator.SetInitialBounds(mOuterHalfsize, mCurvature);
	if (ioCalculator.UpdateCache(mCenterX, mCenterY, mVx, mVy) &&
		ioCalculator.CalculateF(mA, mB))
	{
		ioCalculator.AdjustRectExceptFront();
		ioCalculator.AdjustRectExceptBack();
		if (wasCutAtConcaveXi)
		{
			ioCalculator.CutAtTheConcaveXi();
		}

		if (ioCalculator.ApproximateByParabola(true) &&
			ioCalculator.GetLastApproximationEpsilon() < kApproximationPrecision)
		{
			ioCalculator.SummarizeABAndCalculateCurvature(mA, mB, mCurvature);
			MoveByXi(ioCalculator.GetDeltaXi());
			CalculateTangentVector();
			return FitCenterline(ioCalculator);
		}
	}
	return false;
}

bool ContourDot::PlaceAtNeighborPosition(Calculator& ioCalculator, 
	bool inMoveForward)
{
	float deltaEta = inMoveForward ? 2 * mOuterHalfsize : -2 * mOuterHalfsize;
	float prevX = mCenterX;
	float prevY = mCenterY;
	float prevVx = mVx;
	float prevVy = mVy;
	float prevCurvature = mCurvature;

	bool result = false;
	float referenceVx = mVx;
	float referenceVy = mVy;


	float angle = deltaEta * mCurvature;
	float shrinkFactor = 1 - 0.3f * ::fabs(angle);
	angle *= shrinkFactor;
	deltaEta *= shrinkFactor;

	float etaOfNeighborDot = -deltaEta;
	float c = ::cos(angle);
	float s = ::sin(angle);
	bool isUsingAlternativeBounds = false;

	int tryNo = 0;
	for (; tryNo < 2 && !result; ++tryNo)
	{
		float eta0 = deltaEta - 0.5f * (13 * angle * angle * deltaEta / 35 +
			(13 * prevCurvature - 22 * mCurvature) * angle * deltaEta * deltaEta / 210 +
			((mCurvature * mCurvature + prevCurvature * prevCurvature) / 105 -
			 prevCurvature * mCurvature / 70) * deltaEta * deltaEta * deltaEta);

		float xi0 = (prevCurvature - mCurvature) * deltaEta * deltaEta / 12;

		if (0 != angle)
		{
			xi0 += deltaEta * (1 - c) / angle;
		}

		mCenterX = prevX + prevVx * eta0 - prevVy * xi0;
		mCenterY = prevY + prevVy * eta0 + prevVx * xi0;

		if (0 == tryNo)
		{
			mVx = prevVx * c - prevVy * s;
			mVy = prevVx * s + prevVy * c;
			mA = mVx * mVx - mVy * mVy;
			mB = 2 * mVx * mVy;
		}
		ioCalculator.SetInitialBounds(mOuterHalfsize * shrinkFactor, mCurvature);

		if (!ioCalculator.UpdateCache(mCenterX, mCenterY, mVx, mVy) ||
			!ioCalculator.CalculateF(mA, mB))
		{
			return false;
		}

		if (inMoveForward)
		{
			ioCalculator.AdjustRectExceptBack();
		}
		else
		{
			ioCalculator.AdjustRectExceptFront();
		}
		
		if (!ioCalculator.CheckAlignmentOfMaxPixel(-deltaEta))
		{
			// probably jump between parallel nearby contour lines
			ioCalculator.FindAlternativeXiBounds();
			isUsingAlternativeBounds = true;
		}
		
		if (0 < tryNo)
		{
			ioCalculator.CutAtTheConcaveXi();
		}

		ioCalculator.ResetEtaCalculations();
		result = ioCalculator.ApproximateByParabola(true);
		if (result)
		{
			if (ioCalculator.GetLastApproximationEpsilon() < kApproximationPrecisionSmall ||
				(0 < tryNo && ioCalculator.GetLastApproximationEpsilon() < kApproximationPrecision))
			{
				break;
			}
		}
		ioCalculator.SummarizeABAndCalculateCurvature(mA, mB, mCurvature);
		CalculateTangentVector();
		c = mVx * prevVx + mVy * prevVy;
		s = prevVx * mVy - prevVy * mVx;
		angle = ::atan2(s, c);
		isUsingAlternativeBounds = false;
		result = false;
	}

	if (!result)
	{
		return false;
	}
	ioCalculator.SummarizeABAndCalculateCurvature(mA, mB, mCurvature);
	float newHalfsize = ioCalculator.GetDotHalfsize();
	if (0 >= newHalfsize)
	{
		return false;
	}
		
	// the size of dot have changed, we should accomodate eta-position
	deltaEta = inMoveForward ? 
		newHalfsize - mOuterHalfsize : mOuterHalfsize - newHalfsize;
	float curvedDeltaXi = 0.5f * mCurvature * deltaEta * deltaEta;

	MoveByXi(ioCalculator.GetDeltaXi() + curvedDeltaXi);
	MoveByEta(deltaEta);

	mOuterHalfsize = newHalfsize;
	CalculateTangentVector();

	ioCalculator.SetInitialBounds(mOuterHalfsize, mCurvature);
	if (!ioCalculator.UpdateCache(mCenterX, mCenterY, mVx, mVy) ||
		!ioCalculator.CalculateF(mA, mB))
	{
		return false;
	}

	if (isUsingAlternativeBounds &&
		!ioCalculator.CheckAlignmentOfMaxPixel(etaOfNeighborDot - deltaEta))
	{
		// probably jump between parallel nearby contour lines
		ioCalculator.FindAlternativeXiBounds();
	}

	if (inMoveForward)
	{
		ioCalculator.AdjustRectExceptBack();
	}
	else
	{
		ioCalculator.AdjustRectExceptFront();
	}

	if (0 < tryNo)
	{
		ioCalculator.CutAtTheConcaveXi();
	}

	result = ioCalculator.ApproximateByParabola(true);
	if (!result ||
		ioCalculator.GetLastApproximationEpsilon() > kApproximationPrecisionSmall)
	{
		if (0 == tryNo)
		{
			ioCalculator.CutAtTheConcaveXi();
			result = ioCalculator.ApproximateByParabola(true) &&
				ioCalculator.GetLastApproximationEpsilon() < kApproximationPrecision;
		}
		else if (result)
		{
			result = (ioCalculator.GetLastApproximationEpsilon() < kApproximationPrecision);
		}

		if (!result)
		{
			return false;
		}
	}

	if (OverrideToken token = ioCalculator.HandleOverrides(mCenterX, mCenterY))
	{
		if (kOverrideOtherStroke == token)
		{
			return false;
		}
		if (kOverrideBeyondPictureBounds == token)
		{
			ioCalculator.SummarizeABAndCalculateCurvature(mA, mB, mCurvature);
			mCurvature = 0;
			MoveByXi(ioCalculator.GetDeltaXi());
			mOuterHalfsize = ioCalculator.GetDotHalfsize();
			ioCalculator.GetXiBounds(mMinXi, mMaxXi);
			CalculateTangentVector();
			return true;
		}
	}
	MoveByXi(ioCalculator.GetDeltaXi());

	result = FitCenterline(ioCalculator);
	if (result && 0 > mVx * referenceVx + mVy * referenceVy)
	{
		mVx = -mVx;
		mVy = -mVy;
	}
	return result;
}

///////////////////////////////////////////////////////////////////

bool ContourDot::FitCenterline(Calculator& ioCalculator)
{
	bool result = false;

	const float kWanderLimit = 0.5f * mOuterHalfsize;
	float wander = 0;

	const int kLimitAgainstEternalLoop = 10;
	float moveDistance = 0;
	float previousMoveDistance = 0;

	for (int i = kLimitAgainstEternalLoop; i--;)
	{
		bool success;

		previousMoveDistance = moveDistance;
		moveDistance = ApproachCenterline(ioCalculator, success);
		wander += moveDistance;
		if (!success || ::fabs(wander) > kWanderLimit)
		{
			return false;
		}
	
		if (::fabs(moveDistance) < kPrecision)
		{
			ioCalculator.GetXiBounds(mMinXi, mMaxXi);	
			result = true;
			break;
		}
	}

	// in the case of etenal loop the dot position oscilates, and 
	// we have approximately: moveDistance = -previousMoveDistance
	if (!result &&
		::fabs(moveDistance + previousMoveDistance) < kPrecision)
	{
		float halfMove = 0.5f * moveDistance;
		MoveByXi(-halfMove);
		ioCalculator.GetXiBounds(mMinXi, mMaxXi);
		mMinXi += halfMove;
		mMaxXi += halfMove;
		result = true;
	}

	if (result)
	{
		UpdateAllParameters(ioCalculator);
	}

	return result;
}



float ContourDot::ApproachCenterline(Calculator& ioCalculator, bool& outSuccess)
{
	float deltaXi = 0;
	bool success = false;

	ioCalculator.ExtendXiBounds();
	if (ioCalculator.UpdateCache(mCenterX, mCenterY, mVx, mVy) && 
		ioCalculator.CalculateF(mA, mB))
	{
		ioCalculator.AdjustXiBounds();
		if (ioCalculator.ApproximateByParabola(false))
		{
			deltaXi = ioCalculator.GetDeltaXi();
			ioCalculator.SummarizeAB(mA, mB);
			MoveByXi(deltaXi);
			CalculateTangentVector();
			success = true;
		}
	}
	outSuccess = success;
	return deltaXi;
}

//////////////////////////////////////////////////////////////////
float ContourDot::GetX() const			{ return mCenterX; }
float ContourDot::GetY() const			{ return mCenterY; }
float ContourDot::GetVx() const		{ return mVx; }
float ContourDot::GetVy() const		{ return mVy; }
float ContourDot::GetOuterHalfsize() const { return mOuterHalfsize; }
float ContourDot::GetApproximationPrecision() const	{ return mApproximationPrecision; }
float ContourDot::GetBrightness() const {return mBrightness;}
float ContourDot::GetLineWidth() const { return mLineWidth; }
float ContourDot::GetCurvature() const { return mCurvature; }
float ContourDot::GetMinXi() const { return mMinXi; }
float ContourDot::GetMaxXi() const { return mMaxXi; }

void ContourDot::AddOuterShapeToGraphicsPath(Gdiplus::GraphicsPath& outPath) const
{
	AddXiCurveToGraphicsPath(outPath, -mOuterHalfsize, mOuterHalfsize, -mOuterHalfsize);
	AddEtaLineToGraphicsPath(outPath, mOuterHalfsize);
	AddXiCurveToGraphicsPath(outPath, mOuterHalfsize, -mOuterHalfsize, mOuterHalfsize);
	outPath.CloseFigure();
}

void ContourDot::AddInnerShapeToGraphicsPath(Gdiplus::GraphicsPath& outPath,
	float inMinEta, float inMaxEta) const
{
	AddXiCurveToGraphicsPath(outPath, inMinEta, inMaxEta, mMinXi);
	outPath.StartFigure();
	AddXiCurveToGraphicsPath(outPath, inMinEta, inMaxEta, mMaxXi);
}

void ContourDot::DrawSideColors(Gdiplus::Graphics& outGraphics,
	float inMinEta, float inMaxEta) const
{
	const Gdiplus::ARGB opaqueAlpha = 0xFF000000;
	if (mMinXi < 0)
	{
		Gdiplus::GraphicsPath leftAreaPath;
		AddXiCurveToGraphicsPath(leftAreaPath, inMinEta, inMaxEta, mMinXi);
		AddXiCurveToGraphicsPath(leftAreaPath, inMaxEta, inMinEta, 0.0f);
		leftAreaPath.CloseFigure();
		Gdiplus::Color color(GetSideColorARGB(0) + opaqueAlpha);
		Gdiplus::SolidBrush solidBrush(color);

		Gdiplus::Brush* brush = solidBrush.Clone();
		outGraphics.FillPath(brush, &leftAreaPath);
		delete brush;
	}
	if (mMaxXi > 0)
	{
		Gdiplus::GraphicsPath rightAreaPath;
		AddXiCurveToGraphicsPath(rightAreaPath, inMinEta, inMaxEta, mMaxXi);
		AddXiCurveToGraphicsPath(rightAreaPath, inMaxEta, inMinEta, 0.0f);
		rightAreaPath.CloseFigure();
		Gdiplus::Color color(GetSideColorARGB(3) + opaqueAlpha);
		Gdiplus::SolidBrush solidBrush(color);

		Gdiplus::Brush* brush = solidBrush.Clone();
		outGraphics.FillPath(brush, &rightAreaPath);
		delete brush;
	}
}

//////////////////////////////////////////////////////////////////
void ContourDot::Finalize(Calculator&)
{
	// the last chance to extract any useful information from gradient
	mBrightness = ::sqrt(mM);
	// ....

	// adjust coordinates - the gradient is shifted by the half pixel 
	// against the original bitmap
	mCenterX += 0.5;
	mCenterY += 0.5;

	// compensate an inward shift of blurring the corners
	// (it is visible only at high curvatures)
	float deltaXi = -float(5 * 64 / (9 * M_PI * M_PI * 72)) * 
		mLineWidth * mLineWidth * mCurvature;
	mMinXi -= deltaXi;
	mMaxXi -= deltaXi;
	MoveByXi(deltaXi);
}

bool ContourDot::CanBeNeighbor(const ContourDot& inOtherDot) const
{
	if (0 >= mVx * inOtherDot.GetVx() + mVy * inOtherDot.GetVy())
	{
		return false;
	}

	float distanceLimit = mOuterHalfsize + inOtherDot.GetOuterHalfsize();
	float dx = mCenterX - inOtherDot.GetX();
	float dy = mCenterY - inOtherDot.GetY();
	if (dx * dx + dy * dy > 2.0f * distanceLimit * distanceLimit)
	{
		return false;
	}

	float estimatedRotation = 0.5f * (mCurvature + inOtherDot.GetCurvature())
		* (mVx * dx + mVy * dy);
	float vectorProduct = mVx * inOtherDot.GetVy() - mVy * inOtherDot.GetVx();

	// the rotation and vector product should have the opposite signs 
	// (the exception is a case of very small curvature)
	return estimatedRotation * vectorProduct < 0.05f;
}


bool ContourDot::CalculateTangentVector()
{
	mM = ::sqrt(mA * mA + mB * mB);

	if (mM == 0)
	{
		return false;
	}
	float oldVx = mVx;
	float oldVy = mVy;

	if (mA >= 0)
	{
		mVx = mVx >= 0 ? ::sqrt(0.5f * (mM + mA) / mM) : 
			-::sqrt(0.5f * (mM + mA) / mM);
		mVy = mVx * mB / (mM + mA);
	}
	else
	{
		mVy = mVy >= 0 ? ::sqrt(0.5f * (mM - mA) / mM) :
			-::sqrt(0.5f * (mM - mA) / mM);
		mVx = mVy * mB / (mM - mA);
	}
	if (0 > oldVx * mVx + oldVy * mVy)
	{
		mVx = -mVx;
		mVy = -mVy;
	}

	return true;
}


////////////////////////////////////////////////////////////////////////////////////
// new functions
void ContourDot::UpdateAllParameters(Calculator& ioCalculator)
{
	ioCalculator.ApproximateByParabola(true);
	mApproximationPrecision = ioCalculator.GetLastApproximationEpsilon();
	mLineWidth = ioCalculator.GetLineWidth();

	// the center of Robert's cross is shifted by 0.5 in both coordinates
	// that is deltaX = 0.5 and deltaY = 0.5
	// we should subtract that shift vector from the normal vector n = (-vy, vx)
	// which is directed along the xi axis.
	float xiShift = 0.5f * (mVx - mVy);

	ioCalculator.SummarizeSideColors(xiShift, mSideColorsComponents);
	ioCalculator.SummarizeABAndCalculateCurvature(mA, mB, mCurvature);
	CalculateTangentVector();
}

void ContourDot::MoveByXi(float inXi)
{
	mCenterX -= inXi * mVy;
	mCenterY += inXi * mVx;
}

void ContourDot::MoveByEta(float inEta)
{
	mCenterX += inEta * mVx;
	mCenterY += inEta * mVy;
}

void ContourDot::AddXiCurveToGraphicsPath(Gdiplus::GraphicsPath& outPath,
		float inMinEta, float inMaxEta, float inConstXi) const
{
	float halfCurvature = 0.5f * mCurvature;
	float eta1_3 = (2 * inMinEta + inMaxEta) / 3;
	float eta2_3 = (inMinEta + 2 * inMaxEta) / 3;

	Gdiplus::PointF ptA = MapToPoint(
		inConstXi + halfCurvature * inMinEta * inMinEta, inMinEta);
	Gdiplus::PointF ptM1 = MapToPoint(
		inConstXi + halfCurvature * inMinEta * eta2_3, eta1_3);
	Gdiplus::PointF ptM2 = MapToPoint(
		inConstXi + halfCurvature * inMaxEta * eta1_3, eta2_3);
	Gdiplus::PointF ptB = MapToPoint(
		inConstXi + halfCurvature * inMaxEta * inMaxEta, inMaxEta);
	outPath.AddBezier(ptA, ptM1, ptM2, ptB);
}

void ContourDot::AddEtaLineToGraphicsPath(Gdiplus::GraphicsPath& outPath,
		float inEta) const
{
	float deltaXi = 0.5f * mCurvature * inEta * inEta;
	Gdiplus::PointF pt1 = MapToPoint(-mOuterHalfsize + deltaXi, inEta);
	Gdiplus::PointF pt2 = MapToPoint(mOuterHalfsize + deltaXi, inEta);

	outPath.AddLine(pt1, pt2);
}

inline
Gdiplus::PointF ContourDot::MapToPoint(float inXi, float inEta) const
{
	return Gdiplus::PointF(mCenterX + mVx * inEta - mVy * inXi, 
		mCenterY + mVy * inEta + mVx * inXi);
}

DWORD ContourDot::GetSideColorARGB(int inOffsetToColor) const
{
	const short int* pi = mSideColorsComponents + inOffsetToColor;
	DWORD result = 0;

	// the red color is the first to be shifted-in 
	for (int i = 0; i < 3; ++i)
	{
		result <<= 8;

		// clip the color value into the range 0..255
		result += std::min(255, std::max((int)pi[i], 0));
	}
	return result;
}